#include <iostream>

#include "motor_ctrl.h"

using namespace std;

Motor_Ctrl* g_motor_ctrl;

Motor_Ctrl::Motor_Ctrl(Com_Ctrl *p_com_ctrl, uint8_t num,  Motor_ID_Info array[]) 
{
    cout<<"Motor_Ctrl"<<endl;
    this->p_com_ctrl = p_com_ctrl;

    num = ((num > MOTOR_NUM_MAX) ? MOTOR_NUM_MAX : num);
    this->num = num;
    for(uint8_t i = 0; i < num; i++) {
        if(MOTOR_TYPE_AK == array[i].type) {
            p_motor[i] = new Motor_AK(array[i]);
        } else if(MOTOR_TYPE_YBT == array[i].type){
            p_motor[i] = new Motor_YBT(array[i]);
        } else if(MOTOR_TYPE_VESC == array[i].type){
            p_motor[i] = new Motor_VESC(array[i]);
        }
    }
}

Motor_Ctrl::~Motor_Ctrl() 
{
    cout<<"~Motor_Ctrl"<<endl;
    for(uint8_t i = 0; i < num; i++) {
        delete p_motor[i];
    }
}

int Motor_Ctrl::motor_func(uint8_t num, uint8_t no[], Motor_Mode mode[], double* value[], uint8_t len[])
{
// #define ONEBYONE    1
    int32_t data_len;
    uint8_t mo;
    uint8_t no_index;
    uint8_t dataframe[8];

    if(0 == p_com_ctrl->is_ready()) {
        return -1;
    }
    //USBCAN和UDPCAN最底层走的都是can帧
    if((p_com_ctrl->get_com_type() == COM_USBCAN) || (p_com_ctrl->get_com_type() == COM_UDPCAN)) {
        CAN_Frame_Desc *header = new CAN_Frame_Desc[num];
        for(uint8_t index=0; index<num; index++) {
            no_index = no[index];
            mo = p_motor[no_index]->get_motor_mode_map(mode[index]);
            header[index].RTR   = CAN_RTR_DATA;
            header[index].Group = p_motor[no_index]->get_motor_group();
            header[index].Chn   = p_motor[no_index]->get_motor_chn();
            if(p_motor[no_index]->get_motor_type() == MOTOR_TYPE_AK) {
                header[index].ExtId = p_motor[no_index]->get_motor_id() | ((uint16_t)mo<<8);
                header[index].IDE = CAN_ID_EXT;
            } else if((p_motor[no_index]->get_motor_type() == MOTOR_TYPE_VESC)) {
                header[index].ExtId = p_motor[no_index]->get_motor_id() | ((uint16_t)mo<<8);
                header[index].IDE = CAN_ID_EXT;
            } else if(p_motor[no_index]->get_motor_type() == MOTOR_TYPE_YBT) {
                header[index].StdId = p_motor[no_index]->get_motor_id();
                header[index].IDE = CAN_ID_STD;
            } else {
                return -1;
            }

            data_len = p_motor[no_index]->set_motor_dataframe(dataframe, mode[index], value[index], len[index]);
            if(data_len > 0) {
                header[index].DLC = data_len;
                for(uint8_t i = 0; i < data_len; i++) {
                    header[index].Data[i] = dataframe[i];
                }
            } else if(data_len == 0) {
                header[index].DLC = 0;
            } else {
                cout << "set_motor_dataframe error!" << endl;
                return -1;
            }
    #ifdef ONEBYONE
            p_com_ctrl->send_func(header+index);
        }
    #else
        }
        p_com_ctrl->send_func(header, num);
    #endif
        delete header;
    }  else {
        return -1;
    }
    return 0;
}

int Motor_Ctrl::motor_ctrl_init() 
{
    int ret = 0;
    if((nullptr == p_com_ctrl) || (0 == p_com_ctrl->is_ready())) {
        cout << "can is not ready" << endl;
        return -1;
    }
    for(uint8_t i = 0; i < this->num; i++) {
         ret = p_motor[i]->init();
         if(-1 == ret) {
            return -1;
         }
    }
    return ret;
}

int Motor_Ctrl::motor_ctrl_deinit() 
{
    int ret = 0;
    if((nullptr == p_com_ctrl) || (0 == p_com_ctrl->is_ready())) {
        cout << "can is not ready" << endl;
        return -1;
    }
    for(uint8_t i = 0; i < this->num; i++) {
        ret = p_motor[i]->deinit();
        if(-1 == ret) {
            return -1;
        }
    }
    return ret;
}

Motor::Motor(Motor_ID_Info &info)
{
    std::cout<<"Motor"<<std::endl;
    this->motor_info = info;
}

Motor::~Motor()
{
    std::cout<<"~Motor"<<std::endl;
}

void Motor::set_motor_none()
{
    this->pos = 0;
    this->w   = 0;
    this->t   = 0;
    this->acc = 0;
    this->kp  = 0;
    this->kd  = 0;   
}

int32_t Motor::init()
{
    set_motor_none();
    return 0;
}

int32_t Motor::deinit()
{
    set_motor_none();
    return 0;
}
